#ifndef CERES_OPTIMIZER_H
#define CERES_OPTIMIZER_H
#include <iostream>
#include <vector>

#include "math_utils.h"
#include "state.h"
#include "cost_function.h"

class CeresOptimizer {
public:  
   static void ComputeResidualAndNumericalJacobian(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji);
   static void ComputeResidualAndAutoJacobian(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji);

   static void OptimizeGN(State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity);

// private:
    static void ExponentialFitting();
};

#endif 
